#levi_jia 激光色块找角点
from maix import image, camera, display,touchscreen,uart,time
import threading
cam = camera.Camera(640, 480)
import struct

cam.luma(35)		    # 设置亮度，范围[0, 100]
cam.constrast(40)		# 设置对比度，范围[0, 100]
cam.saturation(60)		# 设置饱和度，范围[0, 100]
cam.exp_mode(0)

device = "/dev/ttyS0"
serial0 = uart.UART(device, 115200)

disp = display.Display()
ts = touchscreen.TouchScreen()
img = image.Image(disp.width(), disp.height())
img = img.lens_corr(strength=1.5)	
count = 0
# line1,line2,line3,line4=[1],[1],[1],[1]
A=1
count = 0 
thresholds3 =[[0, 14, -2, 16, -10, 8],[54, 85, 8, 46, -5, 0],[76, 92, -17, -6, -1, 9]]
label = "BOT"
size = image.string_size(label)
# my_btn_pos = [0, 0, 8*2 + size.width(), 12 * 2 + size.height()]
# my_btn_disp_pos =image.resize_map_pos(img.width(), img.height(), disp.width(), disp.height(), image.Fit.FIT_CONTAIN, my_btn_pos[0], my_btn_pos[1], my_btn_pos[2], my_btn_pos[3])
cx2=0
cy2=0
sline1 = [None]
sline2 = [None]
sline3 = [None]
sline4 = [None]


def is_in_button(x, y, btn_pos):
    return x > btn_pos[0] and x < btn_pos[0] + btn_pos[2] and y > btn_pos[1] and y < btn_pos[1] + btn_pos[3]

def send_four_uchar(c1,c2,c3,c4,c5,c6,c7,c8,c9,c0):
    sentdata = struct.pack("<BiiiiiiiiiiB",
                   0xFE,
                   c1,
                   c2,
                   c3,
                   c4,
                   c5,
                   c6,
                   c7,
                   c8,
                   c9,
                   c0,
                   0xFF
                   )
    serial0.write(sentdata)
    # print(sentdata)
    unpacked_data = struct.unpack("<BiiiiiiiiiiB", sentdata)
    # time.sleep_ms(10)

    print(unpacked_data)


while 1:
    
    img = cam.read()
    img = img.crop(100, 60, 360, 280)

    # img.draw_string(8, 12, label, image.COLOR_WHITE)
    # img.draw_rect(my_btn_pos[0], my_btn_pos[1], my_btn_pos[2], my_btn_pos[3],  image.COLOR_WHITE, 2)
    x, y, pressed = ts.read()
    x, y = image.resize_map_pos_reverse(img.width(), img.height(), disp.width(), disp.height(), image.Fit.FIT_CONTAIN, x, y)
    # if is_in_button(x, y, my_btn_disp_pos)and pressed==1:
    #     print("in boton")
    img.draw_circle(x, y, 1, image.Color.from_rgb(255, 255, 255), 2)


    if A == 1:
        black = [thresholds3[0]]
        red = [thresholds3[1]]
        green = [thresholds3[2]]

        blob1s = img.find_blobs(black, area_threshold=2000,pixels_threshold=10,merge=True)
        # blob2s = img.find_blobs(red, area_threshold=1,pixels_threshold=1,merge=True)
        blob2s = img.find_blobs(red,merge=True)
        for blob1 in blob1s:
                img.draw_rect(blob1[0], blob1[1], blob1[2], blob1[3], image.COLOR_GREEN)#外框
                cx1,cy1 = blob1[0]+blob1[2]/2,blob1[1]+blob1[3]/2
                x1,y1,w1,h1 = blob1[0],blob1[1],blob1[2],blob1[3]
                img.draw_string(blob1[0], 10, str(cx1)+" "+str(cy1), image.Color.from_rgb(255, 255, 0), scale=1)
                img.draw_cross(int(cx1), int(cy1), image.Color.from_rgb(255, 0, 0), size=5, thickness=1)

                corners = blob1.corners()# 计算并打印四个角点
                for corner in corners:
                    img.draw_circle(corner[0], corner[1], 3, image.Color.from_rgb(255, 255, 0)) # 绘制角点
                    # print(corners)
                    if len(corners)>=3:
                        if (corners[1][0]-corners[0][0]==0):
                            slope=90
                        else:
                            slope = (corners[1][1]-corners[0][1])/(corners[1][0]-corners[0][0])
                        # print(slope)
                        img.draw_line(corners[0][0], corners[0][1], corners[1][0], corners[1][1], image.Color.from_rgb(255, 0, 0))
                        img.draw_line(corners[1][0], corners[1][1], corners[2][0], corners[2][1], image.Color.from_rgb(255, 0, 0))
                        img.draw_line(corners[2][0], corners[2][1], corners[3][0], corners[3][1], image.Color.from_rgb(255, 0, 0))
                        img.draw_line(corners[3][0], corners[3][1], corners[0][0], corners[0][1], image.Color.from_rgb(255, 0, 0))
                        count += 1
                        line1=[corners[0][0]+4, corners[0][1], corners[1][0], corners[1][1]+3]
                        line2=[corners[1][0], corners[1][1]+3, corners[2][0]-3, corners[2][1]]
                        line3=[corners[2][0]-3, corners[2][1], corners[3][0], corners[3][1]-3]
                        line4=[corners[3][0], corners[3][1]-3, corners[0][0]+4, corners[0][1]]
                        if count<=100:
                            sline1=line1
                            sline2=line2
                            sline3=line3
                            sline4=line4
                        

        if len(blob2s)<=1 and len(blob2s)>0:
            for blob2 in blob2s:
                # print(blob2)
                # img.draw_rect(blob2[0], blob2[1], blob2[2], blob2[3], image.COLOR_GREEN)#外框
                cx2,cy2 = blob2[0]+blob2[2]/2,blob2[1]+blob2[3]/2
                x2,y2,w2,h2 = blob2[0],blob2[1],blob2[2],blob2[3]
                img.draw_cross(int(cx2), int(cy2), image.Color.from_rgb(255, 255, 255), size=5, thickness=1)
                # print(int(cx2), int(cy2))
                send_four_uchar(int(cx2), int(cy2),sline1[0],sline1[1],sline1[2],sline1[3],sline2[2],sline2[3],sline3[2],sline3[3])
                               
      
        disp.show(img)
    

    
